#region File Description
//-----------------------------------------------------------------------------
// AnimationPlayer.cs
//
// Microsoft XNA Community Game Platform
// Copyright (C) Microsoft Corporation. All rights reserved.
//-----------------------------------------------------------------------------
#endregion

#region Using Statements
using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
#endregion

namespace SkinnedModel
{
    /// <summary>
    /// The animation player is in charge of decoding bone position
    /// matrices from an animation clip.
    /// </summary>
    public class AnimationPlayer
    {
        #region Fields

        // Information about the currently playing animation clip.
        AnimationClip currentClipValue;
        TimeSpan currentTimeValue;
        private int currentKeyframe;
        private int currentPlayCount;
        private float currentPlayScale;
        private int[] currentClipMap;

        // Current animation transform matrices.
        Matrix[] boneTransforms;
        Matrix[] worldTransforms;
        Matrix[] skinTransforms;
        private Matrix[] freezeTransforms;
        private Matrix[] proceduralTransforms;
        private Matrix[] physicsTransforms;

        // Playback mode
        public enum PlaybackMode { Forward, Backward, Partial, LoopForward, LoopBackward, LoopPartial };
        public enum PlaybackStatus { Idle, Start, Repeat, Fade, End };
        private PlaybackMode playbackMode;
        private PlaybackStatus playbackStatus;

        // CrossFade fields
        public enum InterpolationMode { None, Linear, Cubic, Spherical };
        private bool crossFadeEnabled;
        private AnimationClip crossFadeAnimationClip;
        private float crossFadeInterpolationAmount;
        private TimeSpan crossFadeTime;
        private TimeSpan crossFadeElapsedTime;
        private InterpolationMode interpolation;

        // Backlink to the bind pose and skeleton hierarchy data.
        SkinningData skinningDataValue;

        #endregion


        /// <summary>
        /// Constructs a new animation player.
        /// </summary>
        public AnimationPlayer(SkinningData skinningData)
        {
            if (skinningData == null)
                throw new ArgumentNullException("skinningData");

            skinningDataValue = skinningData;
            playbackStatus = PlaybackStatus.Idle;
            interpolation = InterpolationMode.Linear;

            crossFadeEnabled = false;
            crossFadeInterpolationAmount = 0.0f;
            crossFadeTime = TimeSpan.Zero;
            crossFadeElapsedTime = TimeSpan.Zero;

            boneTransforms = new Matrix[skinningData.BindPose.Count];
            worldTransforms = new Matrix[skinningData.BindPose.Count];
            skinTransforms = new Matrix[skinningData.BindPose.Count];
            freezeTransforms = new Matrix[skinningData.BindPose.Count];
            proceduralTransforms = new Matrix[skinningData.BindPose.Count];
            for (int i = 0; i < proceduralTransforms.Length; i++)
                proceduralTransforms[i] = Matrix.Identity;
            physicsTransforms = new Matrix[skinningData.BindPose.Count];
            for (int i = 0; i < physicsTransforms.Length; i++)
                physicsTransforms[i] = Matrix.Identity;
        }

        public Matrix[] BoneTransforms
        {
            get { return boneTransforms; }
        }

        public Matrix[] LocalTransforms
        {
            get { return proceduralTransforms; }
            set { proceduralTransforms = value; }
        }

        public Matrix[] PhysicsTransforms
        {
            get { return physicsTransforms; }
            set { physicsTransforms = value; }
        }

        public InterpolationMode Interpolation
        {
            get { return interpolation; }
            set { interpolation = value; }
        }

        /// <summary>
        /// Starts decoding the specified animation clip.
        /// </summary>
        public void StartClip(AnimationClip clip)
        {
            if (clip == null)
                throw new ArgumentNullException("clip");

            currentClipValue = clip;
            currentTimeValue = TimeSpan.Zero;
            currentKeyframe = 0;
            currentPlayScale = 1.0f;

            // Initialize bone transforms to the bind pose.
            skinningDataValue.BindPose.CopyTo(boneTransforms, 0);
        }

        /// <summary>
        /// Starts decoding the specified animation clip. Use for PlaybackMode.LoopForward/Backward.
        /// </summary>
        /// <param name="clip">Animation clip</param>
        /// <param name="mode">Playback mode</param>
        public void StartClip(AnimationClip clip, PlaybackMode mode)
        {
            StartClip(clip, mode, -1, TimeSpan.Zero, TimeSpan.Zero, false, TimeSpan.Zero);
        }

        /// <summary>
        /// Starts decoding the specified animation clip. Use for PlaybackMode.LoopForward/Backward
        /// with cross fading
        /// </summary>
        /// <param name="clip">Animation clip</param>
        /// <param name="mode">Playback mode</param>
        public void StartClip(AnimationClip clip, PlaybackMode mode, bool fade, TimeSpan duration)
        {
            StartClip(clip, mode, -1, TimeSpan.Zero, TimeSpan.Zero, fade, duration);
        }

        /// <summary>
        /// Starts decoding the specified animation clip. Use for PlaybackMode.Forward/Backward.
        /// </summary>
        /// <param name="clip">Animation clip</param>
        /// <param name="mode">Playback mode</param>
        /// <param name="count">Repeat count</param>
        public void StartClip(AnimationClip clip, PlaybackMode mode, int count)
        {
            StartClip(clip, mode, count, TimeSpan.Zero, TimeSpan.Zero, false, TimeSpan.Zero);
        }

        /// <summary>
        /// Starts decoding the specified animation clip. Use for PlaybackMode.Forward/Backward
        /// with cross fading
        /// </summary>
        /// <param name="clip">Animation clip</param>
        /// <param name="mode">Playback mode</param>
        /// <param name="count">Repeat count</param>
        public void StartClip(AnimationClip clip, PlaybackMode mode, int count, bool fade, TimeSpan duration)
        {
            StartClip(clip, mode, count, TimeSpan.Zero, TimeSpan.Zero, fade, duration);
        }

        public void StartClip(AnimationClip clip, PlaybackMode mode, int count, TimeSpan start, TimeSpan end,
            bool crossFade, TimeSpan duration, float scale, int[] clipMap)
        {
            currentClipMap = clipMap;
            currentPlayScale = scale;
            StartClip(clip, mode, count, start, end, crossFade, duration);
        }

        /// <summary>
        /// Starts a crossfade from current animation clip to target clip.
        /// </summary>
        /// <param name="clip">Target animation clip</param>
        /// <param name="duration">Fade duration</param>
        public void StartClip(AnimationClip clip, PlaybackMode mode, int count, TimeSpan start, TimeSpan end,
            bool crossFade, TimeSpan duration)
        {
            if (crossFade == false)
            {
                if (clip == null)
                    throw new ArgumentNullException("clip");

                currentClipValue = clip;
                currentTimeValue = TimeSpan.Zero;
                currentPlayCount = count;
                playbackMode = mode;

                // Initialize bone transforms to the bind pose.
                skinningDataValue.BindPose.CopyTo(boneTransforms, 0);
            }
            else
            {
                if (crossFadeEnabled)
                {
                    crossFadeInterpolationAmount = 0;
                    crossFadeTime = TimeSpan.Zero;
                    crossFadeElapsedTime = TimeSpan.Zero;

                    StartClip(crossFadeAnimationClip, playbackMode, count, crossFade, duration);
                }

                crossFadeAnimationClip = clip;
                crossFadeTime = duration;
                crossFadeElapsedTime = TimeSpan.Zero;
                crossFadeEnabled = true;
                currentPlayCount = count;
                playbackMode = mode;

                boneTransforms.CopyTo(freezeTransforms, 0);
            }
            playbackStatus = PlaybackStatus.Start;
        }

        /// <summary>
        /// Advances the current animation position.
        /// </summary>
        public void Update(TimeSpan time, bool relativeToCurrentTime,
                           Matrix rootTransform)
        {
            UpdateBoneTransforms(time, relativeToCurrentTime);
            UpdateProceduralTransforms(proceduralTransforms, boneTransforms);
            UpdateWorldTransforms(rootTransform, boneTransforms);
            UpdateSkinTransforms();
        }


        /// <summary>
        /// Helper used by the Update method to refresh the BoneTransforms data.
        /// </summary>
        public void UpdateBoneTransforms(TimeSpan time, bool relativeToCurrentTime)
        {
            if (currentClipValue == null)
                throw new InvalidOperationException(
                            "AnimationPlayer.Update was called before StartClip");

            TimeSpan delta = time;
            Matrix interpolated;

            if (!crossFadeEnabled)
            {
                ///// Update animation time /////
                // Update the animation position.
                TimeSpan clipDuration = new TimeSpan((long)(currentClipValue.Duration.Ticks * currentPlayScale));
                TimeSpan clipExtension = new TimeSpan(0, 0, 0, 0, (int)(1000 * currentPlayScale / 24)); //TODO: fixed on 24fps
                if (relativeToCurrentTime)
                {
                    if (playbackMode == PlaybackMode.LoopForward)
                    {
                        time += currentTimeValue;
                        if (time >= clipDuration + clipExtension)
                        {
                            // If we reached the end, loop back to the start.
                            while (time >= clipDuration + clipExtension)
                                time -= clipDuration + clipExtension;
                            skinningDataValue.BindPose.CopyTo(boneTransforms, 0);
                            playbackStatus = PlaybackStatus.Repeat;
                        }
                    }
                    else if (playbackMode == PlaybackMode.LoopBackward)
                    {
                        time = currentTimeValue - time;
                        if (time <= TimeSpan.Zero)
                        {
                            while (time <= TimeSpan.Zero)
                                time += clipDuration + clipExtension;
                            skinningDataValue.BindPose.CopyTo(boneTransforms, 0);
                            playbackStatus = PlaybackStatus.Repeat;
                        }
                    }
                    else if (playbackMode == PlaybackMode.Forward)
                    {
                        time += currentTimeValue;
                        if (time >= clipDuration + clipExtension)
                        {
                            if (--currentPlayCount > 0)
                            {
                                while (time >= clipDuration + clipExtension)
                                    time -= clipDuration + clipExtension;
                                skinningDataValue.BindPose.CopyTo(boneTransforms, 0);
                                playbackStatus = PlaybackStatus.Repeat;
                            }
                            else
                            {
                                time = clipDuration + clipExtension;
                                playbackStatus = PlaybackStatus.End;
                            }
                        }
                    }
                    else if (playbackMode == PlaybackMode.Backward)
                    {
                        time = currentTimeValue - time;
                        if (time <= TimeSpan.Zero)
                        {
                            if (--currentPlayCount > 0)
                            {
                                while (time <= TimeSpan.Zero)
                                    time += clipDuration + clipExtension;
                                skinningDataValue.BindPose.CopyTo(boneTransforms, 0);
                                playbackStatus = PlaybackStatus.Repeat;
                            }
                            else
                            {
                                time = TimeSpan.Zero;
                                playbackStatus = PlaybackStatus.End;
                            }
                        }
                    }
                    else
                    {
                        throw new InvalidOperationException
                            ("This playback mode is not implemented.");
                    }
                }
                // TODO: no else?
                if ((time < TimeSpan.Zero) || (time > clipDuration+clipExtension))
                    throw new ArgumentOutOfRangeException("time");

                // If the position moved backwards, reset the keyframe index.
                //if (time < currentTimeValue)
                //{
                //    currentKeyframe = 0;
                //    skinningDataValue.BindPose.CopyTo(boneTransforms, 0);
                //}

            }
            else
            {
                ///// Update CrossFade time /////
                if (relativeToCurrentTime)
                    crossFadeElapsedTime += delta;
                // TODO: no else?
                if (crossFadeElapsedTime > crossFadeTime)
                {
                    crossFadeEnabled = false;
                    crossFadeInterpolationAmount = 0;
                    crossFadeTime = TimeSpan.Zero;
                    crossFadeElapsedTime = TimeSpan.Zero;

                    StartClip(crossFadeAnimationClip, playbackMode, currentPlayCount);
                }
                else
                    crossFadeInterpolationAmount = crossFadeElapsedTime.Ticks / (float)crossFadeTime.Ticks;

                playbackStatus = PlaybackStatus.Fade;
            }

            ///// Update current time /////
            currentTimeValue = time;

            if (!crossFadeEnabled)
            {
                IList<Keyframe> keyframes = currentClipValue.Keyframes;
                for (int i = 0; i < currentClipValue.ChannelsList.Count; i++)
                {
                    if ((currentClipMap != null) && (currentClipMap[currentClipValue.ChannelsList[i].Bone] == -1))
                        continue;

                    int channelIndex, curKeyframeIndex, nextKeyframeIndex;
                    IList<int> keyframeIndex = currentClipValue.ChannelsList[i].KeyframeIndex;

                    // find current keyframe accdg to currentTimeValue
                    TimeSpan timeValue = new TimeSpan((long)(currentTimeValue.Ticks / currentPlayScale));
                    channelIndex = GetChannelIndexByTime(currentClipValue, currentClipValue.ChannelsList[i], timeValue);
                    curKeyframeIndex = keyframeIndex[channelIndex];
                    nextKeyframeIndex = keyframeIndex[(channelIndex + 1) % keyframeIndex.Count];

                    // interpolate new transform weighted accdg to currentTimeValue and/or crossfade
                    interpolated = InterpolateTransform(keyframes[curKeyframeIndex], keyframes[nextKeyframeIndex], timeValue);

                    // Use this keyframe
                    //boneTransforms[keyframes[curKeyframeIndex].Bone] = interpolated; // keyframe.Transform;
                    if (currentClipMap != null)
                        boneTransforms[currentClipMap[currentClipValue.ChannelsList[i].Bone]] = interpolated;
                    else
                        boneTransforms[currentClipValue.ChannelsList[i].Bone] = interpolated;
                }
            }
            else
            {

                // loop thru all the bones, not only the active channels
                for (int i = 0; i < boneTransforms.Length; i++)
                {
                    // look for channel in crossFadeClip
                    Keyframe crossFadeKeyframe = null;
                    foreach (Channels chan in crossFadeAnimationClip.ChannelsList)
                    {
                        if (chan.Bone == i)
                        {
                            int crossFadeIndex = GetChannelIndexByTime(crossFadeAnimationClip, chan, TimeSpan.Zero);
                            crossFadeKeyframe = crossFadeAnimationClip.Keyframes[chan.KeyframeIndex[crossFadeIndex]];
                            break;
                        }
                    }
                    if (crossFadeKeyframe != null)
                        interpolated = InterpolateTransform(freezeTransforms[i], crossFadeKeyframe.Transform, crossFadeInterpolationAmount);
                    else
                        interpolated = InterpolateTransform(freezeTransforms[i], skinningDataValue.BindPose[i], crossFadeInterpolationAmount);

                    // Use this keyframe
                    boneTransforms[i] = interpolated; // keyframe.Transform;

                }
            }
        }

        /// <summary>
        /// Interpolates from keyframe to nextKeyframe depending on time
        /// NB. This is unscaled, so comparison is done based on original
        /// time scaling in the animation.
        /// </summary>
        /// <param name="keyframe">Source keyframe</param>
        /// <param name="nextKeyframe">Next keyframe</param>
        /// <param name="time">Current animation time</param>
        /// <returns>Interpolated matrix</returns>
        private Matrix InterpolateTransform(Keyframe keyframe, Keyframe nextKeyframe, TimeSpan time)
        {
            //IList<Keyframe> keyframes = currentClipValue.Keyframes;
            //Keyframe keyframe = keyframes[prevIndex];
            //Keyframe nextKeyframe = keyframes[nextIndex];

            if (interpolation == InterpolationMode.None)
                // no interpolation, just return current keyframe transform
                return keyframe.Transform;
            else
            {
                // calculate interpolation based on duration
                long duration;
                long extension = (long)(1 / 24) * 10000000;     //# of 100ns ticks: TODO
                // TODO: add clip extension to duration based on one frame at 24fps
                if (keyframe.Time > nextKeyframe.Time) // compute duration till end + wrap-around
                    duration = currentClipValue.Duration.Ticks + extension - keyframe.Time.Ticks + nextKeyframe.Time.Ticks;
                else
                    duration = nextKeyframe.Time.Ticks - keyframe.Time.Ticks;

                if (duration > 0)
                {
                    float factor = (time.Ticks - keyframe.Time.Ticks) / (float)(duration);
                    Vector3 currentScale, nextScale, currentTranslate, nextTranslate;
                    Quaternion currentRotate, nextRotate;

                    keyframe.Transform.Decompose(out currentScale, out currentRotate, out currentTranslate);
                    nextKeyframe.Transform.Decompose(out nextScale, out nextRotate, out nextTranslate);

                    if (interpolation == InterpolationMode.Cubic)
                    {
                        // perform cubic transform on scale and translate
                        Vector3 newScale = Vector3.SmoothStep(currentScale, nextScale, factor);
                        Vector3 newTranslate = Vector3.SmoothStep(currentTranslate, nextTranslate, factor);
                        // perform linear transform on rotation
                        Quaternion newRotate = Quaternion.Lerp(currentRotate, nextRotate, factor);

                        return Matrix.CreateScale(newScale) *
                               Matrix.CreateFromQuaternion(newRotate) *
                               Matrix.CreateTranslation(newTranslate);
                    }
                    else if (interpolation == InterpolationMode.Spherical)
                    {
                        // perform linear transform on scale and translation
                        Vector3 newScale = Vector3.Lerp(currentScale, nextScale, factor);
                        Vector3 newTranslate = Vector3.Lerp(currentTranslate, nextTranslate, factor);
                        // perform spherical transform on rotation
                        Quaternion newRotate = Quaternion.Slerp(currentRotate, nextRotate, factor);

                        return Matrix.CreateScale(newScale) *
                               Matrix.CreateFromQuaternion(newRotate) *
                               Matrix.CreateTranslation(newTranslate);
                    }
                    else
                        return Matrix.Lerp(keyframe.Transform, nextKeyframe.Transform, factor);
                }
                else
                    return keyframe.Transform;  // no need for interpolation
            }
        }

        /// <summary>
        /// Interpolates from matrix to nextMatrix depending on weight factor
        /// </summary>
        /// <param name="transform">Source transform</param>
        /// <param name="nextTransform">Next transform</param>
        /// <param name="factor">Blending weight</param>
        /// <returns>Interpolated matrix</returns>
        private Matrix InterpolateTransform(Matrix transform, Matrix nextTransform, float factor)
        {
            if (interpolation == InterpolationMode.None)
            {
                // no interpolation, just return current keyframe transform
                return transform;
            }
            else if (interpolation == InterpolationMode.Cubic)
            {
                Vector3 currentScale, nextScale, currentTranslate, nextTranslate;
                Quaternion currentRotate, nextRotate;

                transform.Decompose(out currentScale, out currentRotate, out currentTranslate);
                nextTransform.Decompose(out nextScale, out nextRotate, out nextTranslate);

                // perform cubic transform on scale and translation
                Vector3 newScale = Vector3.SmoothStep(currentScale, nextScale, factor);
                Vector3 newTranslate = Vector3.SmoothStep(currentTranslate, nextTranslate, factor);
                // perform linear transform on rotation
                Quaternion newRotate = Quaternion.Lerp(currentRotate, nextRotate, factor);

                return Matrix.CreateScale(newScale) *
                       Matrix.CreateFromQuaternion(newRotate) *
                       Matrix.CreateTranslation(newTranslate);
            }
            else if (interpolation == InterpolationMode.Spherical)
            {
                Vector3 currentScale, nextScale, currentTranslate, nextTranslate;
                Quaternion currentRotate, nextRotate;

                transform.Decompose(out currentScale, out currentRotate, out currentTranslate);
                nextTransform.Decompose(out nextScale, out nextRotate, out nextTranslate);

                // perform linear transform on scale and translation
                Vector3 newScale = Vector3.Lerp(currentScale, nextScale, factor);
                Vector3 newTranslate = Vector3.Lerp(currentTranslate, nextTranslate, factor);
                // perform spherical transform on rotation
                Quaternion newRotate = Quaternion.Slerp(currentRotate, nextRotate, factor);

                return Matrix.CreateScale(newScale) *
                       Matrix.CreateFromQuaternion(newRotate) *
                       Matrix.CreateTranslation(newTranslate);
            }
            else
            {
                return Matrix.Lerp(transform, nextTransform, factor);
            }
        }

        /// <summary>
        /// Gets keyframe index from channels list by time
        /// </summary>
        /// <param name="chan">Channels object</param>
        /// <param name="time">Current time</param>
        /// <returns></returns>
        private int GetChannelIndexByTime(AnimationClip clip, Channels chan, TimeSpan time)
        {
            if (chan.KeyframeIndex.Count == 0)
                throw new InvalidOperationException("Empty channel");

            int chanIndex = 0;
            int startIndex = 0;
            int endIndex = chan.KeyframeIndex.Count - 1;

            while (endIndex >= startIndex)
            {
                chanIndex = (startIndex + endIndex) / 2;
                if (clip.Keyframes[chan.KeyframeIndex[chanIndex]].Time < time)
                    startIndex = chanIndex + 1;
                else if (clip.Keyframes[chan.KeyframeIndex[chanIndex]].Time > time)
                    endIndex = chanIndex - 1;
                else
                    break;
            }

            if (clip.Keyframes[chan.KeyframeIndex[chanIndex]].Time > time)
                chanIndex--;

            return chanIndex;
        }

        public void UpdateProceduralTransforms(Matrix[] procTransform, Matrix[] boneTransformsLocal)
        {
            for (int bone = 0; bone < boneTransformsLocal.Length; bone++)
            {
                if (procTransform[bone] != Matrix.Identity)
                    boneTransformsLocal[bone] *= procTransform[bone];
            }
        }


        /// <summary>
        /// Helper used by the Update method to refresh the WorldTransforms data.
        /// </summary>
        public void UpdateWorldTransforms(Matrix rootTransform, Matrix[] boneTransforms)
        {
            // Root bone.
            worldTransforms[0] = boneTransforms[0] * rootTransform;

            // Child bones.
            for (int bone = 1; bone < worldTransforms.Length; bone++)
            {
                int parentBone = skinningDataValue.SkeletonHierarchy[bone];

                worldTransforms[bone] = boneTransforms[bone] *
                                             worldTransforms[parentBone];
            }
        }


        /// <summary>
        /// Helper used by the Update method to refresh the SkinTransforms data.
        /// </summary>
        public void UpdateSkinTransforms()
        {
            for (int bone = 0; bone < skinTransforms.Length; bone++)
            {
                if (physicsTransforms[bone] == Matrix.Identity)
                {
                    skinTransforms[bone] = skinningDataValue.InverseBindPose[bone] *
                                           worldTransforms[bone];

                }
                else
                {
                    /**
                    Vector3 worldScale, scale, worldTranslate, translate;
                    Quaternion rotate;

                    worldTransforms[bone].Decompose(out worldScale, out rotate, out worldTranslate);
                    physicsTransforms[bone].Decompose(out scale, out rotate, out translate);
                    Matrix m = Matrix.CreateScale(worldScale) *
                               Matrix.CreateFromQuaternion(rotate) *
                               Matrix.CreateTranslation(translate);
                    skinTransforms[bone] = skinningDataValue.InverseBindPose[bone] * m;
                     **/
                    skinTransforms[bone] = skinningDataValue.InverseBindPose[bone] *
                                           physicsTransforms[bone];
                }
            }
        }


        /// <summary>
        /// Gets the current bone transform matrices, relative to their parent bones.
        /// </summary>
        public Matrix[] GetBoneTransforms()
        {
            return boneTransforms;
        }

        public void InitBoneTransforms()
        {
            skinningDataValue.BindPose.CopyTo(boneTransforms);
        }


        /// <summary>
        /// Gets the current bone transform matrices, in absolute format.
        /// </summary>
        public Matrix[] GetWorldTransforms()
        {
            return worldTransforms;
        }


        /// <summary>
        /// Gets the current bone transform matrices,
        /// relative to the skinning bind pose.
        /// </summary>
        public Matrix[] GetSkinTransforms()
        {
            return skinTransforms;
        }


        /// <summary>
        /// Gets the clip currently being decoded.
        /// </summary>
        public AnimationClip CurrentClip
        {
            get { return currentClipValue; }
        }


        /// <summary>
        /// Gets the current play position.
        /// </summary>
        public TimeSpan CurrentTime
        {
            get { return currentTimeValue; }
        }

        /// <summary>
        /// Gets the current playback status
        /// </summary>
        public PlaybackStatus CurrentStatus
        {
            get { return playbackStatus; }
        }

    }
}
